#include "CuadrangularObstacle.h"
#include "CuadrangularObstacleSketcher.h"
#include <math.h>
#include "GeometryHelper.h"

CuadrangularObstacle::CuadrangularObstacle(void)
{
	this->setType("CuadrangularObstacle");
	setVertex1(new Position());
	setVertex2(new Position());
	setVertex3(new Position());
	setVertex4(new Position());
}

CuadrangularObstacle::~CuadrangularObstacle(void){
	delete vertex1;
	delete vertex2;
	delete vertex3;
	delete vertex4;
}

//**************************************************************************************************


float CuadrangularObstacle::getMinXPos(){
	return this->getPosition()->getAxisX();
}

float CuadrangularObstacle::getMinYPos(){	
	return this->getPosition()->getAxisY() - this->altura;
}

float CuadrangularObstacle::getMaxXPos(){	
	return this->getPosition()->getAxisX() + this->base;
}

float CuadrangularObstacle::getMaxYPos(){
	return this->getPosition()->getAxisY();
}

void CuadrangularObstacle::setPosition(Position * position){
	this->position = position;
	float cx = SketcherHelper::getInstance()->getCoeficiente();
	this->altura *= cx;
	this->base *= cx;

	getVertex1()->setPixel(position->getPixelX(),position->getPixelY()+(int)altura/2);
	getVertex2()->setPixel(position->getPixelX(),position->getPixelY()-(int)altura/2);
	getVertex3()->setPixel(position->getPixelX()+(int)base,position->getPixelY()+(int)altura/2);
	getVertex4()->setPixel(position->getPixelX()+(int)base,position->getPixelY()-(int)altura/2);
}

Position *CuadrangularObstacle::getPosition(){
	return this->position;
}

Color * CuadrangularObstacle::getElementColor(){
	return this->elementColor;
}

void CuadrangularObstacle::setElementColor(Color * color){
	this->elementColor = color;
}

Texture *CuadrangularObstacle::getTexture(){
	return this->texture;
}

void CuadrangularObstacle::setTexture(Texture *texture){
	this->texture = texture;
}

Color * CuadrangularObstacle::getLineColor()
{
	return this->lineColor;
}

void CuadrangularObstacle::setLineColor( Color * color )
{
	this->lineColor = color;
}

void CuadrangularObstacle::draw()
{
	Sketcher* sketcher = new CuadrangularObstacleSketcher(this);
	sketcher->draw();
	delete(sketcher);
}

void CuadrangularObstacle::setBase( float base )
{
	this->base = base;
}

float CuadrangularObstacle::getBase(){
	return this->base;
}

void CuadrangularObstacle::setAltura( float altura )
{
	this->altura = altura;
}

float CuadrangularObstacle::getAltura(){
	return this->altura;
}

void CuadrangularObstacle::evaluateCollision( Ball * ball ){

	Collision * collision = NULL;

	//* Vertices *************************************************
	collision = evaluateCollisionVertex(ball, vertex1);
	ball->evaluateNextCollision(collision);
	collision = evaluateCollisionVertex(ball, vertex2);
	ball->evaluateNextCollision(collision);
	collision = evaluateCollisionVertex(ball, vertex3);
	ball->evaluateNextCollision(collision);
	collision = evaluateCollisionVertex(ball, vertex4);
	ball->evaluateNextCollision(collision);

	//************************************************************
	try {
		collision = evaluateCollisionEdge(ball,vertex1,vertex2);
		ball->evaluateNextCollision(collision);
	}catch (NoIntersectException e){
	}
	try {
		collision = evaluateCollisionEdge(ball,vertex2,vertex4);
		ball->evaluateNextCollision(collision);
	}catch (NoIntersectException e){
	}
	try {
		collision = evaluateCollisionEdge(ball,vertex3,vertex4);
		ball->evaluateNextCollision(collision);
	}catch (NoIntersectException e){
	}
	try {
		collision = evaluateCollisionEdge(ball,vertex3,vertex1);
		ball->evaluateNextCollision(collision);
	}catch (NoIntersectException e){
	}
}

Collision * CuadrangularObstacle::evaluateCollisionEdge( Ball * ball, Position * point1, Position * point2){
	try {
		float PI = 3.14159;
		float dx = point2->getPixelX()-point1->getPixelX();
		float dy = point2->getPixelY()-point1->getPixelY();
		float r = ball->getRadius();
		float m = ball->getM();
		float b = ball->getPosition()->getPixelY();
		float xb = ball->getPosition()->getPixelX();
		float m2 = dy/dx;
		Position * leftPoint = point1->getPixelX() > point2->getPixelX()? point2 : point1;
		float b2 = leftPoint->getPixelY();
		float xb2 = leftPoint->getPixelX();
		Position * intersection = GeometryHelper::getInstance()->getIntersection(xb,m,b,xb2,m2,b2);
		int signo = GeometryHelper::getInstance()->sign(intersection->getPixelX()-xb);
		float edgeAngle = GeometryHelper::getInstance()->getAngle(m2, signo);
		float ballAngle = ball->getAngle(signo);
		float deltaX,deltaY;

		int minXEdge = min(point1->getPixelX(),point2->getPixelX());
		int maxXEdge = max(point1->getPixelX(),point2->getPixelX());
		int minYEdge = min(point1->getPixelY(),point2->getPixelY());
		int maxYEdge = max(point1->getPixelY(),point2->getPixelY());

		bool existIntersection = false;
		if(point1->getPixelX() != point2->getPixelX()) {
			if(ball->hasImpactSense(intersection)) {
				existIntersection = true;
				float normalAngle = GeometryHelper::getInstance()->correctAngle(edgeAngle - PI / 2,signo);
				float deltaAngle = normalAngle - ballAngle;
				float reflectAngle = normalAngle + deltaAngle;
				deltaX = cos(reflectAngle);
				deltaY = sin(reflectAngle);
			}
		} else {
			if( intersection->getPixelY() < maxYEdge + r && intersection->getPixelY() > minYEdge - r ) {
				if(ball->hasImpactSense(intersection)) {
					existIntersection = true;
					deltaX = cos(-ballAngle);
					deltaY = sin(-ballAngle);
				}
			}
		}
		// Si hay interseccion entre rectas construye la colision.
		Collision *	collision = NULL;
		float distanceUntilImpact = ball->calculateDistance(intersection);
		delete intersection;
		float distanceAdjust = abs(r/sin(ballAngle-edgeAngle));
		float adjustedDistance = distanceUntilImpact - distanceAdjust;
		// Centro de la bola al instante de la colision.
		Position * collisionPoint = ball->calculatePosition(adjustedDistance);
		// Punto de la recta que corresponde a la arista que esta mas cercano al determinado como colision.
		Position * impactPointOnEdge = GeometryHelper::getInstance()->getNearerPoint(collisionPoint,xb2,m2,b2); 
		// Existe interseccion si hay interseccion y el punto de impacto esta en la arista.
		bool existCollision = (existIntersection && minXEdge <= impactPointOnEdge->getPixelX() && impactPointOnEdge->getPixelX() <= maxXEdge && ball->hasImpactSense(collisionPoint));
		if(existCollision) {
			collision = new Collision();
			collision->setDistance(adjustedDistance);
			collision->setPoint(collisionPoint);
			collision->setDeltaX(deltaX);
			collision->setDeltaY(deltaY);
			collision->setElement(static_cast<Element*>(this));
		}
		return collision;
	}catch (ParallelStraightsException e) {
		throw NoIntersectException("La rectas son paralelas.");
	}
	throw NoIntersectException("La recta no intersecta la arista.");
}

Collision * CuadrangularObstacle::evaluateCollisionVertex( Ball * ball, Position * vertex ){

	Position * nearerPoint = GeometryHelper::getInstance()->getNearerPoint(vertex,ball->getPosition()->getPixelX(),ball->getM(),ball->getPosition()->getPixelY());
	float distanceToTrayectory = GeometryHelper::getInstance()->calculateDistance(vertex, nearerPoint);
	if(distanceToTrayectory < ball->getRadius()) {
		float distanceAdjust = ((float)pow((float)ball->getRadius(),2)-(float)pow((float)distanceToTrayectory,2));
		distanceAdjust = pow(distanceAdjust,0.5f);
		float distance = ball->calculateDistance(nearerPoint);
		float adjustedDistance = distance-distanceAdjust;
		Position * collisionPoint = ball->calculatePosition(adjustedDistance);

		if(ball->hasImpactSense(vertex)) {
			Collision * collision = new Collision();
			Position * referencePoint = ball->calculatePosition(adjustedDistance+ball->getRadius());
			float outAngle = GeometryHelper::getInstance()->getAngle(vertex, referencePoint);
			collision->setDeltaX(cos(outAngle));
			collision->setDeltaY(sin(outAngle));
			collision->setPoint(collisionPoint);
			collision->setDistance(adjustedDistance);
			collision->setElement(static_cast<Element*>(this));
			return collision;
		}
	}
	return NULL;
}

Position * CuadrangularObstacle::getVertex1()
{
	return this->vertex1;
}

void CuadrangularObstacle::setVertex1( Position * vertex )
{
	this->vertex1 = vertex;
}

Position * CuadrangularObstacle::getVertex2()
{
	return this->vertex2;
}

void CuadrangularObstacle::setVertex2( Position * vertex )
{
	this->vertex2 = vertex;
}

Position * CuadrangularObstacle::getVertex3()
{
	return this->vertex3;
}

void CuadrangularObstacle::setVertex3( Position * vertex )
{
	this->vertex3 = vertex;
}

Position * CuadrangularObstacle::getVertex4()
{
	return this->vertex4;
}

void CuadrangularObstacle::setVertex4( Position * vertex )
{
	this->vertex4 = vertex;
}